#include "surface_utils.h"
#include "matrix4x4.h"
#include <cmath>

const float SurfaceUtils::RADIO=0.7f;

const float SurfaceUtils::BASE_HEIGHT=0.35f;
const float SurfaceUtils::BASE_WIDTH=0.25f;

void SurfaceUtils::calculateMatrix(Matrix4x4& m,const Point& vecDir,const Point& pos){
	float xyang = atan2(vecDir.x2,vecDir.x1);
	float r = sqrt(pow(vecDir.x1,2)+pow(vecDir.x2,2));
	float xzang = atan2(vecDir.x3,r);
	Matrix4x4 r1 = Matrix4x4(xyang,Point(0.0f,0.0f,1.0f));
	Matrix4x4 r2 = Matrix4x4(xzang,Point(0.0f,1.0f,0.0f));
	Matrix4x4 t3 = Matrix4x4(pos.x1,pos.x2,pos.x3);
	m *= r2;
	m *= r1;
	m *= t3;
}

void SurfaceUtils::calculateCilinderPoints(const Point& vecDir,const Point& point,Points& vectPoints,Points& vectNormal){
	Matrix4x4 t;
	calculateMatrix(t,vecDir,point);

	for(unsigned int i=0;i<CILINDER_ANGLES;i++){
		Point p(0,cos((float)i/(float)(CILINDER_ANGLES-1)*2*PI)*RADIO,sin((float)i/(float)(CILINDER_ANGLES-1)*2*PI)*RADIO);
		p=t*p;
		Point normal=Matrix4x4((float)i/(float)(CILINDER_ANGLES-1)*2*PI,Point(0,1.0,0)) *Point(1,0,0);
		vectPoints.push_back(p);
		vectNormal.push_back(normal);
	}
}

void SurfaceUtils::calculateBasePoints(const Point& vecDir,const Point& point,Points& vectPoints,Points& vectNormal){
	Matrix4x4 t;
	Matrix4x4 e1(1.0f,BASE_WIDTH*2,BASE_HEIGHT*2,true);

	t*=e1; // TODO -> esta deformando la base
	calculateMatrix(t,vecDir,point);


	float y0 = 0.5;
	float x0 = 0.5;
	Point pWall(0,(float)-x0,y0);
	vectPoints.push_back(t*pWall);
	Point nWall(0.0f,1.0f,0.0f);
	vectNormal.push_back(t*nWall);

	for(unsigned int i=0;i<BASE_POINT_COUNT-2;i++){
		// Mapea el intervalo (0,N) a (-x0,x0)
		float x = ((2*(x0))/(float)(BASE_POINT_COUNT-3))*i - x0;
		Point p(0,(float)x,pow((float)x,2));
		Point n=Point(0,0,1)-p;
		p=t*p;
		vectPoints.push_back(p);
		vectNormal.push_back(n);
	}

	Point pWallEnd(0,(float)x0,y0);
	vectPoints.push_back(t*pWallEnd);
	Point nWallEnd(0.0f,-1.0f,0.0f);
	vectNormal.push_back(t*nWallEnd);
}

void SurfaceUtils::calculateIndices(std::vector<unsigned int>& index, unsigned int size,unsigned int lastIndex,Type type){
	unsigned int count;
	switch(type){
		case NONE:
			count=0;
			break;
		case CILINDER:
			count=CILINDER_ANGLES;
			break;
		case BASE:
			count=BASE_POINT_COUNT;
			break;
	}
	for(unsigned int i=1; i<count;i++){
		for(unsigned int j=0;j<size;j++){
			index.push_back((i-1)*size+j+lastIndex);
			index.push_back(i*size+j+lastIndex);
		}
		i++;
		if(i<count){
			for(int k=size-1;k>=0;k--){
				index.push_back((i-1)*size+k+lastIndex);
				index.push_back(i*size+k+lastIndex);
			}
		}
	}
}

void SurfaceUtils::calculateSurfacePoints(const Point& vecDir,const Point& point,Points& vectPoints,Points& vectNormal,Type type){
	switch (type){
		case CILINDER:
			calculateCilinderPoints(vecDir,point,vectPoints,vectNormal);
			break;
		case BASE:
			calculateBasePoints(vecDir,point,vectPoints,vectNormal);
			break;
		default:
			break;
	}
}

unsigned int SurfaceUtils::pointsCountByPlane(Type type){
	switch(type){
		case CILINDER:
			return CILINDER_ANGLES;
		case BASE:
			return BASE_POINT_COUNT;
		default:
			return 0;
	}
	return 0;
}

void SurfaceUtils::sortVectors(	const std::vector<Type>& segmentTypes,unsigned int planeCount,
								Points& vecSurfacePoints,Points& vecNormalSurface){
	Points newVectPoints(vecSurfacePoints.size());
	Points newVectNormalPoints(vecNormalSurface.size());
	size_t iii=vecSurfacePoints.size();
	iii=vecNormalSurface.size();

	Points::const_iterator itPoints=vecSurfacePoints.begin();
	Points::const_iterator itNormal=vecNormalSurface.begin();

	unsigned int total=0;

	for(unsigned int j=0;j<segmentTypes.size();j++){
		while(segmentTypes[j]==0){
			j++;
		}
		if (j<segmentTypes.size()){
			//vertical line es la linea isoparametrica que determina eel numero de plano
			for (unsigned int verticalLine=0;verticalLine<planeCount;verticalLine++){
				unsigned int planePointCount=pointsCountByPlane(segmentTypes[j]);
				for(unsigned int horizontalLine=0;horizontalLine<planePointCount;horizontalLine++){
					newVectPoints.at(total+verticalLine+(planeCount)*horizontalLine)=*itPoints;
					newVectNormalPoints.at(total+verticalLine+(planeCount)*horizontalLine)=*itNormal;
					++itPoints;
					++itNormal;
				}

			}
			total+=planeCount*pointsCountByPlane(segmentTypes[j]);
		}

	}

	vecNormalSurface.assign(newVectNormalPoints.begin(),newVectNormalPoints.end());
	vecSurfacePoints.assign(newVectPoints.begin(),newVectPoints.end());
}
